#include "libmri.h"

#include <omp.h>

#include <algorithm>
using std::max;
using std::min;

void
calib2d_atamul (cplxf *z, cplxf *calib, int *csize, cplxf *x, int *xsize)
{
	int cs1 = csize[0], cs2 = csize[1], nch = csize[2];
	int xs1 = xsize[0], xs2 = xsize[1];

	int m1, m2, m = (m1 = cs1-xs1+1)*(m2 = cs2-xs2+1);
	int n = xs1*xs2*nch;

	cplxf *y = new cplxf [m]; for (int i = 0; i < m; ++i) y[i] = 0.0f;

	for (int ch = 0; ch < nch; ++ch)
	{
		for (int i2 = 0; i2 < m2; ++i2)
			for (int i1 = 0; i1 < m1; ++i1)
			{
				for (int j2 = 0; j2 < xs2; ++j2)
					for (int j1 = 0; j1 < xs1; ++j1)
					{
						int c1 = i1+xs1-1-j1, c2 = i2+xs2-1-j2;

						if (c1 >= 0 and c1 < cs1 and c2 >= 0 and c2 < cs2)
							y[i1 + m1*i2] +=
								calib [c1 + cs1*(c2 + cs2*ch)]
								* x [xs1-1-j1 + xs1*(xs2-1-j2 + xs2*ch)];
					}

			}
	}

	for (int i = 0; i < n; ++i) z[i] = 0.0f;

	for (int ch = 0; ch < nch; ++ch)
	{
		for (int i2 = 0; i2 < xs2; ++i2)
			for (int i1 = 0; i1 < xs1; ++i1)
			{
				for (int j2 = 0; j2 < m2; ++j2)
					for (int j1 = 0; j1 < m1; ++j1) 
					{
						int c1 = i1+m1-1-j1, c2 = i2+m2-1-j2;

						if (c1 >= 0 and c1 < cs1 and c2 >=0 and c2 < cs2)
							z [i1 + xs1*(i2 + xs2*ch)] += 
								conj (calib [c1 + cs1*(c2 + cs2*ch)])
								      * y [m1-1-j1 + m1*(m2-1-j2)];
					}
			}
	}

	//put_shm_array ("y", shm_y);
	delete [] y;
}

#ifdef _MAIN_

int main (int argc, char **argv)
{
	shm_array_t *shm_calib = 0, *shm_x = 0;

	struct arg *args[] = {
		new arg_t <shm_array_t*> ("calib", shm_calib),
		new arg_t <shm_array_t*> ("x", shm_x),
		0};

	get_args (argc, argv, args);

	if (shm_calib == 0){
		if (get_shm_array ("calib", &shm_calib)){
			perror("calib");
			return -1;
		}
	}
	cplxf *calib = (cplxf*) shm_calib->data.c;
	int cs1 = shm_calib->dims[0],
	    cs2 = shm_calib->dims[1],
	    nch = shm_calib->dims[2];

	int csize[3] = {cs1, cs2, nch};

	if (shm_x == 0){
		if (get_shm_array ("x", &shm_x)){
			perror ("x");
			return -1;
		}
	}

	int xs1 = shm_x->dims[0],
	    xs2 = shm_x->dims[1];
	int xsize[2] = {xs1, xs2};
	cplxf *x = (cplxf*) shm_x->data.c;

	shm_array_t z_size, *shm_z = 0;
	z_size.type = shm_array_cplxf;
	z_size.ndims = 3;
	z_size.dims[0] = xs1;
	z_size.dims[1] = xs2;
	z_size.dims[2] = nch;

	if (make_shm_array ("z", &z_size, &shm_z)){
		perror("z");
		return -1;
	}
	cplxf *z = (cplxf*) shm_z->data.c;

	calib2d_atamul (z, calib, csize, x, xsize);

	return 0;
}
#endif
